#pragma config(Motor,  port2,           motor1,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           motor2,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           motor3,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           motor4,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           servo1,        tmotorServoStandard, openLoop)
#pragma config(Motor,  port7,           servo2,        tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void stepServo(tSensors motorPort, int t1)
{
	motor[motorPort] = motor[motorPort] + t1;
  wait1Msec(10);
}

void twoServos (tSensors servoPort1, tSensors servoPort2, int Btn)
{
switch (Btn)
   {
   	case 4:
   	stepServo(servoPort1, 1);
   	break;

   	case 8:
   	stepServo(servoPort2, -1);
   	break;

   	case 16:
   	stepServo(servoPort1, -1);
   	break;

    case 32:
   	stepServo(servoPort2, 1);
   	break;

   	default:
   	wait1Msec(3);
   }
}

int BtnX = 0;

task main()
{
 while(true)
  {
   if (vexRT[Btn5U] == 1){
     BtnX = BtnX + 1;}

   if (vexRT[Btn5D] == 1){
     BtnX = BtnX + 2;}

   if (vexRT[Btn7U] == 1){
     BtnX = BtnX + 4;}

   if (vexRT[Btn7L] == 1){
     BtnX = BtnX + 8;}

   if (vexRT[Btn7D] == 1){
     BtnX = BtnX + 16;}

   if (vexRT[Btn7R] == 1){
     BtnX = BtnX + 32;}

   if (vexRT[Btn6U] == 1){
     BtnX = BtnX + 64;}

   if (vexRT[Btn6D] == 1){
     BtnX = BtnX + 128;}

   if (vexRT[Btn8U] == 1){
     BtnX = BtnX + 256;}

   if (vexRT[Btn8L] == 1){
     BtnX = BtnX + 512;}

   if (vexRT[Btn8D] == 1){
     BtnX = BtnX + 1024;}

   if (vexRT[Btn8R] == 1){
     BtnX = BtnX + 2048;}

   if ((vexRT[Btn7D] == 0)&&(vexRT[Btn7U] == 0)&&(vexRT[Btn7L] == 0)&&(vexRT[Btn7R] == 0)
     &&(vexRT[Btn8D] == 0)&&(vexRT[Btn8U] == 0)&&(vexRT[Btn8L] == 0)&&(vexRT[Btn8R] == 0)
     &&(vexRT[Btn5U] == 0)&&(vexRT[Btn5D] == 0)&&(vexRT[Btn6U] == 0)&&(vexRT[Btn6D] == 0)){
     BtnX = 0;}

   twoServos (in6, in7, BtnX);

   //joystick ch1 controls servo
    if((vexRT[Ch1] <= -40) && (vexRT[Ch1] > -80))
		{
			stepServo(in6, -1);
		}
		else if((vexRT[Ch1] <= -80) && (vexRT[Ch1] > -110))
		{
			stepServo(in6, -2);
		}
		else if(vexRT[Ch1] <= -110)
		{
			stepServo(in6, -3);
		}

		if((vexRT[Ch1] >= 40) && (vexRT[Ch1] < 80))
		{
			stepServo(in6, 1);
		}

		else if((vexRT[Ch1] >= 80) && (vexRT[Ch1] < 110))
		{
			stepServo(in6, 2);
		}
		else if(vexRT[Ch1] >= 110)
		{
			stepServo(in6, 3);
		}
	}
}
